package com.sensePlusVR.setting.bluetooth;


import java.util.LinkedList;
import java.util.Queue;




/**
 * 蓝牙数据处理器
 * @author Charles
 *
 */
public class BTDataProcess {
	

	//TAG
	//private static final String TAG = BTDataProcess.class.getSimpleName();
	
	private static final int BTSourceMax = 21;
	
	//cache最大缓存
	private static final int BTDataCacheMax = 30;
	
	
	
	//原始数据
	private Queue<Double> mBTSource;
	
	//滤波后的数据
	private Queue<Double> mBTFilter;
	
	//数据cache
	private Queue<Double> mBTCache;
	
		
	//最大加速度
	private static final double MaxAccel = -1000.0;
	
	//最小加速度
	private static final double MinAccel = 1000.0;
	
	//波峰
	private double mPeak = 0.0;
	
	//波谷
	private double mTrough = 0.0;
	
	//波峰波谷的落差阈值
	private static final double gapThreshold = 8.0;
	
	//最大深度
	private static final double maxDepth = 1.0;
	
	//方差阈值
	private static final double VarianceThreshold = 3.0;
	
	
	//方差
	private double mVariance = 0.0;
	
	//平均值
	private double mAverage = 0.0;
	
	
	//上次深度
	private double lastDepth = 0.0;
	
	//上次加速度
	private double lastAccelX = 0.0;
	
	//曲线上升
	private boolean lastCurveUp = false;
	
	//频率
	@SuppressWarnings("unused")
	private double freq = 0.0;
	
	//频率阈值
	//private static final float freqThreshold = 8.0f;
	//上次时间
	private long lastTime = 0;
	
	//低通滤波系数
//	private static final double[] nb = new double[]{7.136407237601E-9,1.42728144752E-7,1.355917375144E-6,8.135504250865E-6,3.457589306618E-5,0.0001106428578118,0.0002766071445294,0.0005532142890588,0.0008989732197206, 0.001198630959627,  0.00131849405559, 0.001198630959627,0.0008989732197206,0.0005532142890588,0.0002766071445294,0.0001106428578118,3.457589306618E-5,8.135504250865E-6,1.355917375144E-6,1.42728144752E-7,7.136407237601E-9};
	
//	private static final double[] na = new double[]{0.0,   -7.194091835284,    26.62308164609,   -65.98963230126,121.5313314139,   -175.4744068797,    205.0088928294,   -197.6510906762,159.1947628639,   -107.8907620074,    61.72859748629,   -29.81215019494,12.11309332249,   -4.112484018429,    1.153860006913,   -0.263149759325,0.04759251946719,-0.006572922826265,0.0006516320791281,-4.131947430844E-5,1.259513198204E-6};
	
	
	private static final double[] nb = new double[]{ 2.867384400357e-012,5.734768800714e-011,5.448030360678e-010,3.268818216407e-009,
		  1.389247741973e-008,4.445592774313e-008,1.111398193578e-007,2.222796387157e-007,
		  3.61204412913e-007,4.81605883884e-007,5.297664722723e-007,4.81605883884e-007,
		  3.61204412913e-007,2.222796387157e-007,1.111398193578e-007,4.445592774313e-008,
		  1.389247741973e-008,3.268818216407e-009,5.448030360678e-010,5.734768800714e-011,
		  2.867384400357e-012};
	
	private static final double[] na = new double[]{                0,   -11.99378017162,    69.41165644869,   -257.3693258566,
	      684.9148471655,    -1389.21626524,    2226.486426182,    -2885.15331847,
	      3068.102191284,   -2702.317142789,    1981.137693847,   -1210.501541604,
	      615.1008807534,   -258.4187123042,     88.8556606349,   -24.61260608736,
	      5.361835809716,  -0.8851266245437,   0.1041357771743,-0.007783676685313,
	  0.0002779267153158};
	
	public BTDataProcess(){
		
		mBTCache = new LinkedList<Double>();
		
		mBTSource = new LinkedList<Double>();
		
		mBTFilter = new LinkedList<Double>();
		
	}
	

	
	/**
	 * 获取当前深度 0-1之间
	 * @param accelX
	 * @return
	 */
	public double[] getCurDepth(double accelX,boolean isFilter){
		
		//滤波后数据
		accelX = isFilter?filterData(accelX):accelX;
		
				
		//是否计算波峰波谷
		boolean shouldCalPeakAndTrough = true;
		
		
		if(mBTCache.size()>=BTDataCacheMax){
			
			double head = mBTCache.poll();
			
			if(head!=mPeak&&head!=mTrough){
				
				shouldCalPeakAndTrough = false;
			}
		}
		
		
		
		//计算波峰波谷
		if(shouldCalPeakAndTrough){
			
			calPeakAndTrough();			
		}
		
		
		mBTCache.add(accelX);
		
		if(accelX>mPeak){
			
			mPeak = accelX;
			
		}
		
		if(accelX<mTrough){
			
			mTrough = accelX;
			
		}

		double gap = mPeak - mTrough;
	
		
		double depth = lastDepth;
		
		if(mVariance>VarianceThreshold){
			
				
			
			depth = (accelX - mTrough)/gap;	
				
				
			depth = maxDepth - depth;
			

			
	
		}else{//缓慢起作用
			
			if(gap>gapThreshold)
			{
				depth = (accelX + gap*0.5 - mAverage)/gap;
				
				
				depth = maxDepth - depth;
				
			}
			else
			{
				
				
			}
	
				
			
			
			
		}
		
		depth = depth>1.0?1.0:depth;
		
		depth = depth<0?0.0:depth;

			
		
		boolean curCurveUp = accelX-lastAccelX>=0.0;
		
		if(!curCurveUp&&lastCurveUp)
		{
			long curTime = System.currentTimeMillis();
			
			freq = 1000.0/(curTime-lastTime);
			
			lastTime = curTime;
		}
		
		
	//	Log.e(TAG, "mAVG="+mAverage+" mVariance="+mVariance+" gap="+gap+"freq="+freq);
		
		lastAccelX = accelX;
		
		lastCurveUp = curCurveUp;
		
		lastDepth = depth;
		
		return new double[]{depth,mVariance};
		
		
		
		
	}
	

	/**
	 * 低通滤波后数据
	 * @param accelX
	 * @return
	 */
	private double filterData(double accelX)
	{
		if(mBTSource.size()>=BTSourceMax){
			
			mBTSource.poll();
			
		}
		
		mBTSource.add(accelX);
		
		Object[] arr = mBTSource.toArray();
		
		Object[] fiterArr = mBTFilter.toArray();
		
		
		
		int srcLen = mBTSource.size();
		
	//	int fiterLen = mBTFilter.size();
		
		
		double value = 0.0;
		
		double accelVal = 0.0;
		
		double fiterVal = 0.0;
		
		for(int i = 0;i<srcLen;i++){
			
			if(i!=0)
			{
				fiterVal = ((Double)fiterArr[srcLen-i-1]).doubleValue();	
						
				
			}
			
			accelVal = ((Double)arr[srcLen-i-1]).doubleValue();
			
			
			value = value + nb[i]*accelVal - na[i]*fiterVal;
			
		//	Log.e(TAG, "fiterArr size="+fiterLen+" srcLen="+srcLen+" i="+i+" index="+(srcLen-i-1)+" fiterVal="+fiterVal+" accelVal="+accelVal);
		}
		
	//	Log.e(TAG, "totalValue="+value);
		
		if(mBTFilter.size()>=BTSourceMax-1){
			
			mBTFilter.poll();
			
		}
		mBTFilter.add(value);
		
		
		return value;
	}
	
	
	/**
	 * 计算波峰波谷
	 */
	private void calPeakAndTrough(){
		
		
		
		Object[] arr = mBTCache.toArray();
		
		mPeak = MaxAccel;
		
		mTrough = MinAccel;
		
		mAverage = 0.0;
		
		mVariance = 0.0;
		
		for(Object obj:arr){
					
			double value = ((Double)obj).doubleValue();
			
			mAverage += value;
			
			mVariance += value*value;
			
			if(value>mPeak){
				
				mPeak = value;
			}
			
			if(value<mTrough){
				
				mTrough = value;
			}
		}
		
		mAverage /= arr.length;
		
		mVariance = (mVariance-arr.length*mAverage*mAverage)/arr.length;
		

	}


}
